import rospy
import pyquaternion
from gazebo_msgs.srv import SetModelState, SpawnModel
from gazebo_msgs.msg import ModelState,ModelStates
from math import pi, cos, sin
from geometry_msgs.msg import *
model_state_pub = rospy.Publisher(
    "gazebo/set_model_states", ModelStates, queue_size=10)
rospy.init_node('set_pose')
spawn_model = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel)
item_pose = Pose(Point(x=-20, y=-20, z=0),  Quaternion(0, 0, 0, 1))
sdf_xml = '''
<?xml version="1.0" ?>
<sdf version="1.6">
  <model name="FordPickUp">
    <static>true</static>
    <link name="link">
      <pose>0 0 0 0 0 0</pose>
      <visual name="visual">
        <geometry>
          <mesh>
            <scale>0.7 0.7 0.7</scale>
            <uri>/home/pop/YuanChuangCup/models/FordPickUp/meshes/PickUp.dae</uri>
          </mesh>
        </geometry>
      </visual>
    </link>
  </model>
</sdf>'''

V_NUM = 10
# for num in range(V_NUM):
#     spawn_model("pickup_"+str(num), sdf_xml, "", item_pose, "world")


# print("222")
i=0
while i<10000:
    i = i+0.1
    states_msg = ModelStates()
    # state_msg.reference_frame = "solo"
    for num in range(V_NUM):
        states_msg.pose.append(Pose(Point(x=20*cos(2*pi/2000*i + 2*pi/V_NUM * num), y=20*sin(2*pi/2000*i+ 2*pi/V_NUM * num ), z=10),   pyquaternion.Quaternion(axis=[0, 0, 1], angle=2*pi/2000*i + pi+ 2*pi/V_NUM * num)))
        states_msg.name.append("pickup_"+str(num))
        states_msg.twist.append(Twist(Vector3(0,0,0),Vector3(0,0,0)))

    model_state_pub.publish(states_msg)
        # rospy.sleep(0.001)

    # state_msg.model_name = "solo"
    # state_msg.pose = Pose(Point(x=i/100, y=0, z=1),   pyquaternion.Quaternion(axis=[0, 0, 1], angle=0))
    # model_state_pub.publish()

    rospy.sleep(0.001)
